
/**
  ******************************************************************************
  * Copyright 2021 The Microbee Authors. All Rights Reserved.
  * 
  * Licensed under the Apache License, Version 2.0 (the "License");
  * you may not use this file except in compliance with the License.
  * You may obtain a copy of the License at
  * 
  * http://www.apache.org/licenses/LICENSE-2.0
  * 
  * Unless required by applicable law or agreed to in writing, software
  * distributed under the License is distributed on an "AS IS" BASIS,
  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
  * See the License for the specific language governing permissions and
  * limitations under the License.
  * 
  * @file       ap_log.c
  * @author     baiyang
  * @date       2023-2-23
  ******************************************************************************
  */

/*----------------------------------include-----------------------------------*/
#include "ap_log.h"
#include "blog.h"

#include <posix/string.h>
#include <stdarg.h>
#include <stdio.h>

#include <ahrs/ahrs_view.h>
#include <common/microbee.h>
#include <uITC/uITC_msg.h>
#include <parameter/param.h>
#include <common/time/gp_time.h>
#include <common/location/location.h>
#include <common/gp_math/gp_mathlib.h>
/*-----------------------------------macro------------------------------------*/
#ifndef ARRAY_SIZE
#define ARRAY_SIZE(_arr) (sizeof(_arr)/sizeof(_arr[0]))
#endif
/*----------------------------------typedef-----------------------------------*/
struct ap_log{
    const struct LogStructure *_structure;
    const struct UnitStructure *_units;
    const struct MultiplierStructure *_multipliers;

    aplog_vehicle_startup_message _vehicle_startup_message;

    enum AplogStage stage;

    uint8_t _num_types;
    uint8_t _num_units;
    uint8_t _num_multipliers;

    bool startup_messages_finished;

    Param_uint32 _log_bitmask;
};
/*---------------------------------prototype----------------------------------*/

/*----------------------------------variable----------------------------------*/
struct ap_log aplog;

// all units here should be base units
// This does mean battery capacity is here as "amp*second"
// Please keep the names consistent with Tools/autotest/param_metadata/param.py:33
const struct UnitStructure log_Units[] = {
    { '-', "" },              // no units e.g. Pi, or a string
    { '?', "UNKNOWN" },       // Units which haven't been worked out yet....
    { 'A', "A" },             // Ampere
    { 'a', "Ah" },            // Ampere hours
    { 'd', "deg" },           // of the angular variety, -180 to 180
    { 'b', "B" },             // bytes
    { 'k', "deg/s" },         // degrees per second. Degrees are NOT SI, but is some situations more user-friendly than radians
    { 'D', "deglatitude" },   // degrees of latitude
    { 'e', "deg/s/s" },       // degrees per second per second. Degrees are NOT SI, but is some situations more user-friendly than radians
    { 'E', "rad/s" },         // radians per second
    { 'G', "Gauss" },         // Gauss is not an SI unit, but 1 tesla = 10000 gauss so a simple replacement is not possible here
    { 'h', "degheading" },    // 0.? to 359.?
    { 'i', "A.s" },           // Ampere second
    { 'J', "W.s" },           // Joule (Watt second)
    { 'l', "l" },             // litres
    { 'L', "rad/s/s" },       // radians per second per second
    { 'm', "m" },             // metres
    { 'n', "m/s" },           // metres per second
    // { 'N', "N" },          // Newton
    { 'o', "m/s/s" },         // metres per second per second
    { 'O', "degC" },          // degrees Celsius. Not SI, but Kelvin is too cumbersome for most users
    { '%', "%" },             // percent
    { 'S', "satellites" },    // number of satellites
    { 's', "s" },             // seconds
    { 'q', "rpm" },           // rounds per minute. Not SI, but sometimes more intuitive than Hertz
    { 'r', "rad" },           // radians
    { 'U', "deglongitude" },  // degrees of longitude
    { 'u', "ppm" },           // pulses per minute
    { 'v', "V" },             // Volt
    { 'P', "Pa" },            // Pascal
    { 'w', "Ohm" },           // Ohm
    { 'W', "Watt" },          // Watt
    { 'X', "W.h" },           // Watt hour
    { 'y', "l/s" },           // litres per second
    { 'Y', "us" },            // pulse width modulation in microseconds
    { 'z', "Hz" },            // Hertz
    { '#', "instance" }       // (e.g.)Sensor instance number
};

// this multiplier information applies to the raw value present in the
// log.  Any adjustment implied by the format field (e.g. the "centi"
// in "centidegrees" is *IGNORED* for the purposes of scaling.
// Essentially "format" simply tells you the C-type, and format-type h
// (int16_t) is equivalent to format-type c (int16_t*100)
// tl;dr a GCS shouldn't/mustn't infer any scaling from the unit name

const struct MultiplierStructure log_Multipliers[] = {
    { '-', 0 },       // no multiplier e.g. a string
    { '?', 1 },       // multipliers which haven't been worked out yet....
// <leave a gap here, just in case....>
    { '2', 1e2 },
    { '1', 1e1 },
    { '0', 1e0 },
    { 'A', 1e-1 },
    { 'B', 1e-2 },
    { 'C', 1e-3 },
    { 'D', 1e-4 },
    { 'E', 1e-5 },
    { 'F', 1e-6 },
    { 'G', 1e-7 },
    { 'I', 1e-9 },
// <leave a gap here, just in case....>
    { '!', 3.6 }, // (ampere*second => milliampere*hour) and (km/h => m/s)
    { '/', 3600 }, // (ampere*second => ampere*hour)
};
/*-------------------------------------os-------------------------------------*/

/*----------------------------------function----------------------------------*/
/*
  strncpy without the warning for not leaving room for nul termination
 */
static void strncpy_noterm(char *dest, const char *src, size_t n)
{
    size_t len = strnlen(src, n);
    if (len < n) {
        // include nul term if it fits
        len++;
    }
    memcpy(dest, src, len);
}

void aplog_init(const struct LogStructure *structure, uint16_t len)
{
    aplog.startup_messages_finished = false;

    aplog._structure = structure;
    aplog._num_types = len;

    aplog._units     = &log_Units[0];
    aplog._num_units = ARRAY_SIZE(log_Units);

    aplog._multipliers     = &log_Multipliers[0];
    aplog._num_multipliers = ARRAY_SIZE(log_Multipliers);

    param_link_variable(PARAM_ID(VEHICLE, LOG_BITMASK), &aplog._log_bitmask);
}

bool aplog_should_log(const uint32_t mask)
{
    if (!(mask & aplog._log_bitmask)) {
        return false;
    }

    if (blog_get_status() != BLOG_STATUS_LOGGING) {
        return false;
    }

    return true;
}

void aplog_start()
{
    aplog.stage = APLOG_FORMATS;
    aplog.startup_messages_finished = false;
}

void aplog_stop()
{
    aplog.startup_messages_finished = true;
}

void aplog_write_more_startup_messages()
{
    if (aplog.startup_messages_finished) {
        return;
    }

    switch(aplog.stage) {
        case APLOG_FORMATS:
            for(uint16_t i = 0; i < aplog._num_types; i++)
            {
                aplog_write_format(&aplog._structure[i]);
            }
            blog_flush();
            aplog.stage = APLOG_UNITS;
            break;

        case APLOG_UNITS:
            for(uint16_t i = 0; i < aplog._num_units; i++)
            {
                aplog_write_unit(&aplog._units[i]);
            }
            blog_flush();
            aplog.stage = APLOG_MULTIPLIERS;
            break;

        case APLOG_MULTIPLIERS:
            for(uint16_t i = 0; i < aplog._num_multipliers; i++)
            {
                aplog_write_multiplier(&aplog._multipliers[i]);
            }
            blog_flush();
            aplog.stage = APLOG_FORMAT_UNITS;
            break;

        case APLOG_FORMAT_UNITS:
            for(uint16_t i = 0; i < aplog._num_types; i++)
            {
                aplog_write_format_units(&aplog._structure[i]);
            }
            blog_flush();
            aplog.stage = APLOG_PARMS;
            break;

        case APLOG_PARMS: {
            param_t* parameter = param_get_first();
            while (parameter != NULL) {
                aplog_write_parameter(parameter->name, param_cast_to_float(parameter), parameter->default_val);
                parameter = param_get_next_scalar(parameter);
            }
            blog_flush();

            aplog.stage = APLOG_RUNNING_SUBWRITERS;
            break;
        }

        case APLOG_RUNNING_SUBWRITERS:
            aplog.stage = APLOG_VEHICLE_MESSAGES;
            break;

        case APLOG_VEHICLE_MESSAGES:
            if (aplog_write_vehicle_messages()) {
                aplog.stage = APLOG_DONE;
            }
            blog_flush();
            break;

        case APLOG_DONE:
            aplog.startup_messages_finished = true;
            break;
    }
}

// Write a text message to the log
bool aplog_write_message(const char *message)
{
    struct log_Message pkt = {
        APLOG_PACKET_HEADER_INIT(LOG_MESSAGE_MSG),
        .time_us = time_micros64(),
        .msg  = {0}
    };
    strncpy_noterm(pkt.msg, message, sizeof(pkt.msg));
    return blog_write_critical_block(&pkt, sizeof(pkt));
}

void aplog_write_messageF(const char *fmt, ...)
{
    char msg[65] = {0}; // sizeof(log_Message.msg) + null-termination

    va_list ap;
    va_start(ap, fmt);
    vsnprintf(msg, sizeof(msg), fmt, ap);
    va_end(ap);

    aplog_write_message(msg);
}

void aplog_fill_format(const struct LogStructure *structure, struct log_Format *pkt)
{
    memset(pkt, 0, sizeof(struct log_Format));
    pkt->head1 = APLOG_HEAD_BYTE1;
    pkt->head2 = APLOG_HEAD_BYTE2;
    pkt->msgid = LOG_FORMAT_MSG;
    pkt->type = structure->msg_type;
    pkt->length = structure->msg_len;
    strncpy_noterm(pkt->name, structure->name, sizeof(pkt->name));
    strncpy_noterm(pkt->format, structure->format, sizeof(pkt->format));
    strncpy_noterm(pkt->labels, structure->labels, sizeof(pkt->labels));
}

void aplog_fill_format_units(const struct LogStructure *s, struct log_Format_Units *pkt)
{
    memset(pkt, 0, sizeof(struct log_Format_Units));
    pkt->head1 = APLOG_HEAD_BYTE1;
    pkt->head2 = APLOG_HEAD_BYTE2;
    pkt->msgid = LOG_FORMAT_UNITS_MSG;
    pkt->time_us = time_micros64();
    pkt->format_type = s->msg_type;
    strncpy_noterm(pkt->units, s->units, sizeof(pkt->units));
    strncpy_noterm(pkt->multipliers, s->multipliers, sizeof(pkt->multipliers));
}

/*
  write a structure format to the log
 */
bool aplog_write_format(const struct LogStructure *s)
{
    struct log_Format pkt;
    aplog_fill_format(s, &pkt);
    return aplog.startup_messages_finished ? blog_write_critical_block(&pkt, sizeof(pkt)) : blog_write_file(&pkt, sizeof(pkt));
}

/*
  write a unit definition
 */
bool aplog_write_unit(const struct UnitStructure *s)
{
    struct log_Unit pkt = {
        APLOG_PACKET_HEADER_INIT(LOG_UNIT_MSG),
        .time_us = time_micros64(),
        .type    = s->ID,
        .unit    = {0}
    };
    strncpy_noterm(pkt.unit, s->unit, sizeof(pkt.unit));

    return aplog.startup_messages_finished ? blog_write_critical_block(&pkt, sizeof(pkt)) : blog_write_file(&pkt, sizeof(pkt));
}

/*
  write a unit-multiplier definition
 */
bool aplog_write_multiplier(const struct MultiplierStructure *s)
{
    const struct log_Format_Multiplier pkt = {
        APLOG_PACKET_HEADER_INIT(LOG_MULT_MSG),
        .time_us      = time_micros64(),
        .type         = s->ID,
        .multiplier   = s->multiplier,
    };

    return aplog.startup_messages_finished ? blog_write_critical_block(&pkt, sizeof(pkt)) : blog_write_file(&pkt, sizeof(pkt));
}

/*
  write the units for a format to the log
 */
bool aplog_write_format_units(const struct LogStructure *s)
{
    struct log_Format_Units pkt;
    aplog_fill_format_units(s, &pkt);
    return aplog.startup_messages_finished ? blog_write_critical_block(&pkt, sizeof(pkt)) : blog_write_file(&pkt, sizeof(pkt));
}

/*
  write a parameter to the log
 */
bool aplog_write_parameter(const char *name, float value, float default_val)
{
    struct log_Parameter pkt = {
        APLOG_PACKET_HEADER_INIT(LOG_PARAMETER_MSG),
        .time_us = time_micros64(),
        .name  = {0},
        .value = value,
        .default_value = default_val
    };
    strncpy_noterm(pkt.name, name, sizeof(pkt.name));
    return aplog.startup_messages_finished ? blog_write_critical_block(&pkt, sizeof(pkt)) : blog_write_file(&pkt, sizeof(pkt));
}

void aplog_set_vehicle_startup_writer(aplog_vehicle_startup_message writer)
{
    aplog._vehicle_startup_message = writer;
}

/*
  write a parameter to the log
 */
bool aplog_write_vehicle_messages()
{
    if (aplog._vehicle_startup_message != NULL) {
        aplog._vehicle_startup_message();
        return true;
    }

    return false;
}

///
/**
  * @brief       记录IMU数据
  * @param[in]
  * @param[in]
  * @param[out]
  * @retval
  * @note
  */
void aplog_write_imu()
{
    uitc_sensor_gyr sensor_gyr;
    uitc_sensor_acc sensor_acc;

    if ((itc_copy_from_hub(ITC_ID(sensor_gyr_raw1), &sensor_gyr) == 0) &&
        (itc_copy_from_hub(ITC_ID(sensor_acc_raw1), &sensor_acc) == 0)) {
        const struct log_IMU pkt = {
             APLOG_PACKET_HEADER_INIT(LOG_IMU_MSG),
            .time_us  = sensor_gyr.timestamp_us,
            .instance = 0,
            .gyro_x   = sensor_gyr.sensor_gyr_filter[0],
            .gyro_y   = sensor_gyr.sensor_gyr_filter[1],
            .gyro_z   = sensor_gyr.sensor_gyr_filter[2],
            .accel_x  = sensor_acc.sensor_acc_filter[0],
            .accel_y  = sensor_acc.sensor_acc_filter[1],
            .accel_z  = sensor_acc.sensor_acc_filter[2],
            .gyro_error  = sensor_gyr.gyro_error,
            .accel_error = sensor_acc.accel_gyro_error,
            .temperature = sensor_acc.temperature,
            .gyro_health = sensor_gyr.healthy,
            .accel_health = sensor_acc.healthy,
            .gyro_rate = sensor_gyr.rate_hz,
            .accel_rate = sensor_acc.rate_hz,
        };

        blog_write_critical_block(&pkt, sizeof(pkt));

        aplog_write_gyr(0, sensor_gyr.timestamp_us, sensor_gyr.sensor_gyr_correct);
        aplog_write_acc(0, sensor_acc.timestamp_us, sensor_acc.sensor_acc_correct);
    }

    if ((itc_copy_from_hub(ITC_ID(sensor_gyr_raw2), &sensor_gyr) == 0) &&
        (itc_copy_from_hub(ITC_ID(sensor_acc_raw2), &sensor_acc) == 0)) {
        const struct log_IMU pkt = {
             APLOG_PACKET_HEADER_INIT(LOG_IMU_MSG),
            .time_us  = sensor_gyr.timestamp_us,
            .instance = 1,
            .gyro_x   = sensor_gyr.sensor_gyr_filter[0],
            .gyro_y   = sensor_gyr.sensor_gyr_filter[1],
            .gyro_z   = sensor_gyr.sensor_gyr_filter[2],
            .accel_x  = sensor_acc.sensor_acc_filter[0],
            .accel_y  = sensor_acc.sensor_acc_filter[1],
            .accel_z  = sensor_acc.sensor_acc_filter[2],
            .gyro_error  = sensor_gyr.gyro_error,
            .accel_error = sensor_acc.accel_gyro_error,
            .temperature = sensor_acc.temperature,
            .gyro_health = sensor_gyr.healthy,
            .accel_health = sensor_acc.healthy,
            .gyro_rate = sensor_gyr.rate_hz,
            .accel_rate = sensor_acc.rate_hz,
        };

        blog_write_critical_block(&pkt, sizeof(pkt));

        aplog_write_gyr(1, sensor_gyr.timestamp_us, sensor_gyr.sensor_gyr_correct);
        aplog_write_acc(1, sensor_acc.timestamp_us, sensor_acc.sensor_acc_correct);
    }

    if ((itc_copy_from_hub(ITC_ID(sensor_gyr_raw3), &sensor_gyr) == 0) &&
        (itc_copy_from_hub(ITC_ID(sensor_acc_raw3), &sensor_acc) == 0)) {
        const struct log_IMU pkt = {
             APLOG_PACKET_HEADER_INIT(LOG_IMU_MSG),
            .time_us  = sensor_gyr.timestamp_us,
            .instance = 2,
            .gyro_x   = sensor_gyr.sensor_gyr_filter[0],
            .gyro_y   = sensor_gyr.sensor_gyr_filter[1],
            .gyro_z   = sensor_gyr.sensor_gyr_filter[2],
            .accel_x  = sensor_acc.sensor_acc_filter[0],
            .accel_y  = sensor_acc.sensor_acc_filter[1],
            .accel_z  = sensor_acc.sensor_acc_filter[2],
            .gyro_error  = sensor_gyr.gyro_error,
            .accel_error = sensor_acc.accel_gyro_error,
            .temperature = sensor_acc.temperature,
            .gyro_health = sensor_gyr.healthy,
            .accel_health = sensor_acc.healthy,
            .gyro_rate = sensor_gyr.rate_hz,
            .accel_rate = sensor_acc.rate_hz,
        };

        blog_write_critical_block(&pkt, sizeof(pkt));

        aplog_write_gyr(2, sensor_gyr.timestamp_us, sensor_gyr.sensor_gyr_correct);
        aplog_write_acc(2, sensor_acc.timestamp_us, sensor_acc.sensor_acc_correct);
    }
}

// Write ACC data packet: raw accel data
void aplog_write_acc(const uint8_t instance, const uint64_t sample_us, const float accel[3])
{
    const uint64_t now = time_micros64();
    const struct log_ACC pkt = {
        APLOG_PACKET_HEADER_INIT(LOG_ACC_MSG),
        .time_us   = now,
        .instance  = instance,
        .sample_us = sample_us?sample_us:now,
        .AccX      = accel[0],
        .AccY      = accel[1],
        .AccZ      = accel[2]
    };
    blog_write_critical_block(&pkt, sizeof(pkt));
}

// Write GYR data packet: raw gyro data
void aplog_write_gyr(const uint8_t instance, const uint64_t sample_us, const float gyro[3])
{
    const uint64_t now = time_micros64();
    const struct log_GYR pkt = {
        APLOG_PACKET_HEADER_INIT(LOG_GYR_MSG),
        .time_us   = now,
        .instance  = instance,
        .sample_us = sample_us?sample_us:now,
        .GyrX      = gyro[0],
        .GyrY      = gyro[1],
        .GyrZ      = gyro[2]
    };
    blog_write_critical_block(&pkt, sizeof(pkt));
}

void aplog_write_imu_fast()
{
    uitc_sensor_gyr sensor_gyr;
    uitc_sensor_acc sensor_acc;

    if ((itc_copy_from_hub(ITC_ID(sensor_gyr_raw1), &sensor_gyr) == 0) &&
        (itc_copy_from_hub(ITC_ID(sensor_acc_raw1), &sensor_acc) == 0)) {
        const struct log_IMU pkt = {
             APLOG_PACKET_HEADER_INIT(LOG_IMU_MSG),
            .time_us  = sensor_gyr.timestamp_us,
            .instance = 0,
            .gyro_x   = sensor_gyr.sensor_gyr_filter[0],
            .gyro_y   = sensor_gyr.sensor_gyr_filter[1],
            .gyro_z   = sensor_gyr.sensor_gyr_filter[2],
            .accel_x  = sensor_acc.sensor_acc_filter[0],
            .accel_y  = sensor_acc.sensor_acc_filter[1],
            .accel_z  = sensor_acc.sensor_acc_filter[2],
            .gyro_error  = sensor_gyr.gyro_error,
            .accel_error = sensor_acc.accel_gyro_error,
            .temperature = sensor_acc.temperature,
            .gyro_health = sensor_gyr.healthy,
            .accel_health = sensor_acc.healthy,
            .gyro_rate = sensor_gyr.rate_hz,
            .accel_rate = sensor_acc.rate_hz,
        };

        blog_write_critical_block(&pkt, sizeof(pkt));

        aplog_write_gyr(0, sensor_gyr.timestamp_us, sensor_gyr.sensor_gyr_correct);
        aplog_write_acc(0, sensor_acc.timestamp_us, sensor_acc.sensor_acc_correct);
    }

    if ((itc_copy_from_hub(ITC_ID(sensor_gyr_raw2), &sensor_gyr) == 0) &&
        (itc_copy_from_hub(ITC_ID(sensor_acc_raw2), &sensor_acc) == 0)) {
        const struct log_IMU pkt = {
             APLOG_PACKET_HEADER_INIT(LOG_IMU_MSG),
            .time_us  = sensor_gyr.timestamp_us,
            .instance = 1,
            .gyro_x   = sensor_gyr.sensor_gyr_filter[0],
            .gyro_y   = sensor_gyr.sensor_gyr_filter[1],
            .gyro_z   = sensor_gyr.sensor_gyr_filter[2],
            .accel_x  = sensor_acc.sensor_acc_filter[0],
            .accel_y  = sensor_acc.sensor_acc_filter[1],
            .accel_z  = sensor_acc.sensor_acc_filter[2],
            .gyro_error  = sensor_gyr.gyro_error,
            .accel_error = sensor_acc.accel_gyro_error,
            .temperature = sensor_acc.temperature,
            .gyro_health = sensor_gyr.healthy,
            .accel_health = sensor_acc.healthy,
            .gyro_rate = sensor_gyr.rate_hz,
            .accel_rate = sensor_acc.rate_hz,
        };

        blog_write_critical_block(&pkt, sizeof(pkt));

        aplog_write_gyr(1, sensor_gyr.timestamp_us, sensor_gyr.sensor_gyr_correct);
        aplog_write_acc(1, sensor_acc.timestamp_us, sensor_acc.sensor_acc_correct);
    }

    if ((itc_copy_from_hub(ITC_ID(sensor_gyr_raw3), &sensor_gyr) == 0) &&
        (itc_copy_from_hub(ITC_ID(sensor_acc_raw3), &sensor_acc) == 0)) {
        const struct log_IMU pkt = {
             APLOG_PACKET_HEADER_INIT(LOG_IMU_MSG),
            .time_us  = sensor_gyr.timestamp_us,
            .instance = 2,
            .gyro_x   = sensor_gyr.sensor_gyr_filter[0],
            .gyro_y   = sensor_gyr.sensor_gyr_filter[1],
            .gyro_z   = sensor_gyr.sensor_gyr_filter[2],
            .accel_x  = sensor_acc.sensor_acc_filter[0],
            .accel_y  = sensor_acc.sensor_acc_filter[1],
            .accel_z  = sensor_acc.sensor_acc_filter[2],
            .gyro_error  = sensor_gyr.gyro_error,
            .accel_error = sensor_acc.accel_gyro_error,
            .temperature = sensor_acc.temperature,
            .gyro_health = sensor_gyr.healthy,
            .accel_health = sensor_acc.healthy,
            .gyro_rate = sensor_gyr.rate_hz,
            .accel_rate = sensor_acc.rate_hz,
        };

        blog_write_critical_block(&pkt, sizeof(pkt));

        aplog_write_gyr(2, sensor_gyr.timestamp_us, sensor_gyr.sensor_gyr_correct);
        aplog_write_acc(2, sensor_acc.timestamp_us, sensor_acc.sensor_acc_correct);
    }
}

// Write VIBE data packet for all instances
void aplog_write_vibration()
{
    uitc_vehicle_vibration vehicle_vibration;

    if (itc_copy_from_hub(ITC_ID(vehicle_vibration1), &vehicle_vibration) == 0) {
        const struct log_Vibe pkt = {
            APLOG_PACKET_HEADER_INIT(LOG_VIBE_MSG),
            .time_us     = vehicle_vibration.timestamp_us,
            .imu         = 0,
            .vibe_x      = vehicle_vibration.vibe_x,
            .vibe_y      = vehicle_vibration.vibe_y,
            .vibe_z      = vehicle_vibration.vibe_z,
            .clipping    = vehicle_vibration.clipping
        };

        blog_write_critical_block(&pkt, sizeof(pkt));
    }

    if (itc_copy_from_hub(ITC_ID(vehicle_vibration2), &vehicle_vibration) == 0) {
        const struct log_Vibe pkt = {
            APLOG_PACKET_HEADER_INIT(LOG_VIBE_MSG),
            .time_us     = vehicle_vibration.timestamp_us,
            .imu         = 1,
            .vibe_x      = vehicle_vibration.vibe_x,
            .vibe_y      = vehicle_vibration.vibe_y,
            .vibe_z      = vehicle_vibration.vibe_z,
            .clipping    = vehicle_vibration.clipping
        };

        blog_write_critical_block(&pkt, sizeof(pkt));
    }

    if (itc_copy_from_hub(ITC_ID(vehicle_vibration3), &vehicle_vibration) == 0) {
        const struct log_Vibe pkt = {
            APLOG_PACKET_HEADER_INIT(LOG_VIBE_MSG),
            .time_us     = vehicle_vibration.timestamp_us,
            .imu         = 2,
            .vibe_x      = vehicle_vibration.vibe_x,
            .vibe_y      = vehicle_vibration.vibe_y,
            .vibe_z      = vehicle_vibration.vibe_z,
            .clipping    = vehicle_vibration.clipping
        };

        blog_write_critical_block(&pkt, sizeof(pkt));
    }
}

/**
  * @brief       
  * @param[in]     
  * @param[out]  
  * @retval      
  * @note        
  */
void aplog_write_baro()
{
    uitc_sensor_baro_raw sensor_baro_raw;

    if (itc_copy_from_hub(ITC_ID(sensor_baro_raw1), &sensor_baro_raw) == 0) {
        const struct log_BARO pkt = {
            APLOG_PACKET_HEADER_INIT(LOG_BARO_MSG),
            .time_us        = time_micros64(),
            .instance       = 0,
            .altitude       = sensor_baro_raw.altitude,
            .pressure       = sensor_baro_raw.pressure_pa,
            .temperature    = (int16_t)(sensor_baro_raw.temperature_deg * 100 + 0.5f),
            .climbrate      = sensor_baro_raw.climbrate,
            .sample_time_ms = sensor_baro_raw.timestamp_us*0.001f,
            .drift_offset   = sensor_baro_raw.alt_offset_active,
            .ground_temp    = sensor_baro_raw.ground_temp,
            .healthy        = (uint8_t)sensor_baro_raw.healthy,
        };

        blog_write_critical_block(&pkt, sizeof(pkt));
    }

    if (itc_copy_from_hub(ITC_ID(sensor_baro_raw2), &sensor_baro_raw) == 0) {
        const struct log_BARO pkt = {
            APLOG_PACKET_HEADER_INIT(LOG_BARO_MSG),
            .time_us        = time_micros64(),
            .instance       = 1,
            .altitude       = sensor_baro_raw.altitude,
            .pressure       = sensor_baro_raw.pressure_pa,
            .temperature    = (int16_t)(sensor_baro_raw.temperature_deg * 100 + 0.5f),
            .climbrate      = sensor_baro_raw.climbrate,
            .sample_time_ms = sensor_baro_raw.timestamp_us*0.001f,
            .drift_offset   = sensor_baro_raw.alt_offset_active,
            .ground_temp    = sensor_baro_raw.ground_temp,
            .healthy        = (uint8_t)sensor_baro_raw.healthy,
        };

        blog_write_critical_block(&pkt, sizeof(pkt));
    }

    if (itc_copy_from_hub(ITC_ID(sensor_baro_raw3), &sensor_baro_raw) == 0) {
        const struct log_BARO pkt = {
            APLOG_PACKET_HEADER_INIT(LOG_BARO_MSG),
            .time_us        = time_micros64(),
            .instance       = 2,
            .altitude       = sensor_baro_raw.altitude,
            .pressure       = sensor_baro_raw.pressure_pa,
            .temperature    = (int16_t)(sensor_baro_raw.temperature_deg * 100 + 0.5f),
            .climbrate      = sensor_baro_raw.climbrate,
            .sample_time_ms = sensor_baro_raw.timestamp_us*0.001f,
            .drift_offset   = sensor_baro_raw.alt_offset_active,
            .ground_temp    = sensor_baro_raw.ground_temp,
            .healthy        = (uint8_t)sensor_baro_raw.healthy,
        };

        blog_write_critical_block(&pkt, sizeof(pkt));
    }
}

/**
  * @brief       记录磁罗盘数据
  * @param[in]
  * @param[out]
  * @retval
  * @note
  */
void aplog_write_mag()
{
    uitc_sensor_mag sensor_mag;

    if (itc_copy_from_hub(ITC_ID(sensor_mag_raw1), &sensor_mag) == 0) {
        const struct log_MAG pkt = {
            APLOG_PACKET_HEADER_INIT(LOG_MAG_MSG),
            .time_us         = sensor_mag.timestamp_us,
            .instance        = 0,
            .mag_x           = (int16_t)(sensor_mag.sensor_mag_correct[0]),
            .mag_y           = (int16_t)(sensor_mag.sensor_mag_correct[1]),
            .mag_z           = (int16_t)(sensor_mag.sensor_mag_correct[2]),
            .offset_x        = (int16_t)0,
            .offset_y        = (int16_t)0,
            .offset_z        = (int16_t)0,
            .motor_offset_x  = (int16_t)0,
            .motor_offset_y  = (int16_t)0,
            .motor_offset_z  = (int16_t)0,
            .health          = (uint8_t)sensor_mag.healthy,
            .SUS             = sensor_mag.last_update_usec
        };

        blog_write_critical_block(&pkt, sizeof(pkt));
    }

    if (itc_copy_from_hub(ITC_ID(sensor_mag_raw2), &sensor_mag) == 0) {
        const struct log_MAG pkt = {
            APLOG_PACKET_HEADER_INIT(LOG_MAG_MSG),
            .time_us         = sensor_mag.timestamp_us,
            .instance        = 1,
            .mag_x           = (int16_t)(sensor_mag.sensor_mag_correct[0]),
            .mag_y           = (int16_t)(sensor_mag.sensor_mag_correct[1]),
            .mag_z           = (int16_t)(sensor_mag.sensor_mag_correct[2]),
            .offset_x        = (int16_t)0,
            .offset_y        = (int16_t)0,
            .offset_z        = (int16_t)0,
            .motor_offset_x  = (int16_t)0,
            .motor_offset_y  = (int16_t)0,
            .motor_offset_z  = (int16_t)0,
            .health          = (uint8_t)sensor_mag.healthy,
            .SUS             = sensor_mag.last_update_usec
        };

        blog_write_critical_block(&pkt, sizeof(pkt));
    }

    if (itc_copy_from_hub(ITC_ID(sensor_mag_raw3), &sensor_mag) == 0) {
        const struct log_MAG pkt = {
            APLOG_PACKET_HEADER_INIT(LOG_MAG_MSG),
            .time_us         = sensor_mag.timestamp_us,
            .instance        = 2,
            .mag_x           = (int16_t)(sensor_mag.sensor_mag_correct[0]),
            .mag_y           = (int16_t)(sensor_mag.sensor_mag_correct[1]),
            .mag_z           = (int16_t)(sensor_mag.sensor_mag_correct[2]),
            .offset_x        = (int16_t)0,
            .offset_y        = (int16_t)0,
            .offset_z        = (int16_t)0,
            .motor_offset_x  = (int16_t)0,
            .motor_offset_y  = (int16_t)0,
            .motor_offset_z  = (int16_t)0,
            .health          = (uint8_t)sensor_mag.healthy,
            .SUS             = sensor_mag.last_update_usec
        };

        blog_write_critical_block(&pkt, sizeof(pkt));
    }
}

/**
  * @brief       记录gps
  * @param[in]
  * @param[out]
  * @retval
  * @note
  */
void aplog_write_write_gps()
{
    static bool should_log = false;

    uitc_sensor_gps vehicle_gps_position;

    static bool should_log1 = false;
    if (itc_copy_from_hub(ITC_ID(sensor_gps_raw1),  &vehicle_gps_position) == 0) {

        if (!should_log1 && vehicle_gps_position.fix_type > 1) {
            should_log1 = true;
        }

        struct log_GPA pkt2 = {
            APLOG_PACKET_HEADER_INIT(LOG_GPA_MSG),
            .time_us       = time_micros64(),
            .instance      = 0,
            .vdop          = vehicle_gps_position.vdop,
            .hacc          = (uint16_t)MIN((vehicle_gps_position.horizontal_accuracy*100), UINT16_MAX),
            .vacc          = (uint16_t)MIN((vehicle_gps_position.vertical_accuracy*100), UINT16_MAX),
            .sacc          = (uint16_t)MIN((vehicle_gps_position.speed_accuracy*100), UINT16_MAX),
            .yaw_accuracy  = vehicle_gps_position.gps_yaw_accuracy,
            .have_vv       = (uint8_t)vehicle_gps_position.have_vertical_velocity,
            .sample_ms     = vehicle_gps_position.last_message_time_ms,
            .delta_ms      = vehicle_gps_position.delta_time_ms,
            .undulation    = vehicle_gps_position.undulation,
        };
        blog_write_critical_block(&pkt2, sizeof(pkt2));

        if (!should_log1) {
            return;
        }

        const struct log_GPS pkt = {
            APLOG_PACKET_HEADER_INIT(LOG_GPS_MSG),
            .time_us       = vehicle_gps_position.timestamp_us,
            .instance      = 0,
            .status        = vehicle_gps_position.fix_type,
            .gps_week_ms   = vehicle_gps_position.time_week_ms,
            .gps_week      = vehicle_gps_position.time_week,
            .num_sats      =vehicle_gps_position.num_sats,
            .hdop          = vehicle_gps_position.hdop,
            .latitude      = vehicle_gps_position.lat,
            .longitude     = vehicle_gps_position.lon,
            .altitude      = vehicle_gps_position.alt_msl/10,
            .ground_speed  = vehicle_gps_position.vel_m_s,
            .ground_course = vehicle_gps_position.cog_rad,
            .vel_z         = vehicle_gps_position.vel_d_m_s,
            .yaw           = vehicle_gps_position.gps_yaw,
            .used          = vehicle_gps_position.primary_instance
        };
        blog_write_critical_block(&pkt, sizeof(pkt));
    }

    static bool should_log2 = false;
    if (itc_copy_from_hub(ITC_ID(sensor_gps_raw2),  &vehicle_gps_position) == 0) {

        if (!should_log2 && vehicle_gps_position.fix_type > 1) {
            should_log2 = true;
        }

        struct log_GPA pkt2 = {
            APLOG_PACKET_HEADER_INIT(LOG_GPA_MSG),
            .time_us       = time_micros64(),
            .instance      = 1,
            .vdop          = vehicle_gps_position.vdop,
            .hacc          = (uint16_t)MIN((vehicle_gps_position.horizontal_accuracy*100), UINT16_MAX),
            .vacc          = (uint16_t)MIN((vehicle_gps_position.vertical_accuracy*100), UINT16_MAX),
            .sacc          = (uint16_t)MIN((vehicle_gps_position.speed_accuracy*100), UINT16_MAX),
            .yaw_accuracy  = vehicle_gps_position.gps_yaw_accuracy,
            .have_vv       = (uint8_t)vehicle_gps_position.have_vertical_velocity,
            .sample_ms     = vehicle_gps_position.last_message_time_ms,
            .delta_ms      = vehicle_gps_position.delta_time_ms,
            .undulation    = vehicle_gps_position.undulation,
        };
        blog_write_critical_block(&pkt2, sizeof(pkt2));

        if (!should_log2) {
            return;
        }

        const struct log_GPS pkt = {
            APLOG_PACKET_HEADER_INIT(LOG_GPS_MSG),
            .time_us       = vehicle_gps_position.timestamp_us,
            .instance      = 1,
            .status        = vehicle_gps_position.fix_type,
            .gps_week_ms   = vehicle_gps_position.time_week_ms,
            .gps_week      = vehicle_gps_position.time_week,
            .num_sats      =vehicle_gps_position.num_sats,
            .hdop          = vehicle_gps_position.hdop,
            .latitude      = vehicle_gps_position.lat,
            .longitude     = vehicle_gps_position.lon,
            .altitude      = vehicle_gps_position.alt_msl/10,
            .ground_speed  = vehicle_gps_position.vel_m_s,
            .ground_course = vehicle_gps_position.cog_rad,
            .vel_z         = vehicle_gps_position.vel_d_m_s,
            .yaw           = vehicle_gps_position.gps_yaw,
            .used          = vehicle_gps_position.primary_instance
        };
        blog_write_critical_block(&pkt, sizeof(pkt));
    }

    static bool should_log3 = false;
    if (itc_copy_from_hub(ITC_ID(sensor_gps_raw3),  &vehicle_gps_position) == 0) {

        if (!should_log3 && vehicle_gps_position.fix_type > 1) {
            should_log3 = true;
        }

        struct log_GPA pkt2 = {
            APLOG_PACKET_HEADER_INIT(LOG_GPA_MSG),
            .time_us       = time_micros64(),
            .instance      = 2,
            .vdop          = vehicle_gps_position.vdop,
            .hacc          = (uint16_t)MIN((vehicle_gps_position.horizontal_accuracy*100), UINT16_MAX),
            .vacc          = (uint16_t)MIN((vehicle_gps_position.vertical_accuracy*100), UINT16_MAX),
            .sacc          = (uint16_t)MIN((vehicle_gps_position.speed_accuracy*100), UINT16_MAX),
            .yaw_accuracy  = vehicle_gps_position.gps_yaw_accuracy,
            .have_vv       = (uint8_t)vehicle_gps_position.have_vertical_velocity,
            .sample_ms     = vehicle_gps_position.last_message_time_ms,
            .delta_ms      = vehicle_gps_position.delta_time_ms,
            .undulation    = vehicle_gps_position.undulation,
        };
        blog_write_critical_block(&pkt2, sizeof(pkt2));

        if (!should_log3) {
            return;
        }

        const struct log_GPS pkt = {
            APLOG_PACKET_HEADER_INIT(LOG_GPS_MSG),
            .time_us       = vehicle_gps_position.timestamp_us,
            .instance      = 2,
            .status        = vehicle_gps_position.fix_type,
            .gps_week_ms   = vehicle_gps_position.time_week_ms,
            .gps_week      = vehicle_gps_position.time_week,
            .num_sats      =vehicle_gps_position.num_sats,
            .hdop          = vehicle_gps_position.hdop,
            .latitude      = vehicle_gps_position.lat,
            .longitude     = vehicle_gps_position.lon,
            .altitude      = vehicle_gps_position.alt_msl/10,
            .ground_speed  = vehicle_gps_position.vel_m_s,
            .ground_course = vehicle_gps_position.cog_rad,
            .vel_z         = vehicle_gps_position.vel_d_m_s,
            .yaw           = vehicle_gps_position.gps_yaw,
            .used          = vehicle_gps_position.primary_instance
        };
        blog_write_critical_block(&pkt, sizeof(pkt));
    }
}

// Write an attitude packet
void aplog_write_attitude()
{
    uitc_vehicle_attitude vehicle_attitude;
    uitc_sensor_gyr sensor_gyr;
    uitc_vehicle_att_ctrl att_ctrl;
    uitc_vehicle_pos_ctrl_z pos_ctrl_z;

    itc_copy_from_hub(ITC_ID(vehicle_pos_ctrl_z), &pos_ctrl_z);

    if (itc_copy_from_hub(ITC_ID(vehicle_attitude), &vehicle_attitude) == 0 && \
        itc_copy_from_hub(ITC_ID(sensor_gyr), &sensor_gyr) == 0 && \
        itc_copy_from_hub(ITC_ID(vehicle_att_ctrl), &att_ctrl) == 0) {
        const struct log_Attitude pkt = {
            APLOG_PACKET_HEADER_INIT(LOG_ATTITUDE_MSG),
            .time_us         = vehicle_attitude.timestamp_us,
            .control_roll    = (int16_t)degrees(att_ctrl.target_euler_angle[0] * 100),
            .roll            = (int16_t)degrees(vehicle_attitude.vehicle_euler.roll * 100),
            .control_pitch   = (int16_t)degrees(att_ctrl.target_euler_angle[1] * 100),
            .pitch           = (int16_t)degrees(vehicle_attitude.vehicle_euler.pitch * 100),
            .control_yaw     = (uint16_t)math_wrap_360_cd(degrees(att_ctrl.target_euler_angle[2] * 100)),
            .yaw             = (uint16_t)math_wrap_360_cd(degrees(vehicle_attitude.vehicle_euler.yaw * 100)),
            .error_rp        = 0,
            .error_yaw       = 0,
            .active          = 0,
        };
        blog_write_critical_block(&pkt, sizeof(pkt));

        const struct log_Rate pkt_rate = {
            APLOG_PACKET_HEADER_INIT(LOG_RATE_MSG),
            .time_us         = vehicle_attitude.timestamp_us,
            .control_roll    = degrees(att_ctrl.target_ang_vel_body[0]),
            .roll            = degrees(sensor_gyr.sensor_gyr_filter[0]),
            .roll_out        = att_ctrl.roll_out,
            .control_pitch   = degrees(att_ctrl.target_ang_vel_body[1]),
            .pitch           = degrees(sensor_gyr.sensor_gyr_filter[1]),
            .pitch_out       = att_ctrl.pitch_out,
            .control_yaw     = degrees(att_ctrl.target_ang_vel_body[2]),
            .yaw             = degrees(sensor_gyr.sensor_gyr_filter[2]),
            .yaw_out         = att_ctrl.yaw_out,
            .control_accel   = pos_ctrl_z.target_acc_z,
            .accel           = (float)(-(get_ahrs_view()->accel_ef.z + GRAVITY_MSS) * 100.0f),
            .accel_out       = att_ctrl.throttle_out,
            .throttle_slew   = att_ctrl.throttle_slew
        };

        blog_write_critical_block(&pkt_rate, sizeof(pkt_rate));
    }
}

// Write a POS packet
void aplog_write_pos()
{
    Location loc;
    uitc_vehicle_local_position local_position;
    uitc_vehicle_status vehicle_status;

    if (itc_copy_from_hub(ITC_ID(vehicle_local_position), &local_position) == 0
        && local_position.xy_global
        && itc_copy_from_hub(ITC_ID(vehicle_status), &vehicle_status) == 0) {

        Vector3f_t offset_neu = {local_position.x*100, local_position.y*100, -local_position.z*100};
        Location_from_vector_xy_alt(&loc, &offset_neu, ALT_FRAME_ABOVE_ORIGIN);

        const struct log_POS pkt = {
            APLOG_PACKET_HEADER_INIT(LOG_POS_MSG),
            .time_us        = local_position.timestamp,
            .lat            = loc.lat,
            .lng            = loc.lng,
            .alt            = loc.alt*1.0e-2f,
            .rel_home_alt   = (-local_position.z - vehicle_status.arming_altitude_m),
            .rel_origin_alt = -local_position.z,
        };
        blog_write_critical_block(&pkt, sizeof(pkt));
    }
}

void aplog_write_psc_ne()
{
    uitc_vehicle_pos_ctrl_xy vehicle_pos_ctrl_xy;
    uitc_vehicle_local_position local_position;

    if ((itc_copy_from_hub(ITC_ID(vehicle_pos_ctrl_xy), &vehicle_pos_ctrl_xy) == 0 && vehicle_pos_ctrl_xy.is_active_xy)
        && itc_copy_from_hub(ITC_ID(vehicle_local_position), &local_position) == 0) {
        const struct log_PSCN pkt = {
            APLOG_PACKET_HEADER_INIT(LOG_PSCN_MSG),
            .time_us       = vehicle_pos_ctrl_xy.timestamp_us,
            .pos_target    = vehicle_pos_ctrl_xy.target_pos_xy[0] * 0.01f,
            .pos           = local_position.x,
            .vel_desired   = vehicle_pos_ctrl_xy.vel_desired_xy[0] * 0.01f,
            .vel_target    = vehicle_pos_ctrl_xy.target_vel_xy[0] * 0.01f,
            .vel           = local_position.vx,
            .accel_desired = vehicle_pos_ctrl_xy.accel_desired_xy[0] * 0.01f,
            .accel_target  = vehicle_pos_ctrl_xy.target_acc_xy[0] * 0.01f,
            .accel         = local_position.ax
        };
        blog_write_critical_block(&pkt, sizeof(pkt));

        const struct log_PSCE pkt2 = {
            APLOG_PACKET_HEADER_INIT(LOG_PSCE_MSG),
            .time_us       = vehicle_pos_ctrl_xy.timestamp_us,
            .pos_target    = vehicle_pos_ctrl_xy.target_pos_xy[1] * 0.01f,
            .pos           = local_position.y,
            .vel_desired   = vehicle_pos_ctrl_xy.vel_desired_xy[1] * 0.01f,
            .vel_target    = vehicle_pos_ctrl_xy.target_vel_xy[1] * 0.01f,
            .vel           = local_position.vy,
            .accel_desired = vehicle_pos_ctrl_xy.accel_desired_xy[1] * 0.01f,
            .accel_target  = vehicle_pos_ctrl_xy.target_acc_xy[1] * 0.01f,
            .accel         = local_position.ay
        };
        blog_write_critical_block(&pkt2, sizeof(pkt2));
    }
}

void aplog_write_psc_d()
{
    uitc_vehicle_local_position local_position;
    uitc_vehicle_pos_ctrl_z pos_ctrl_z;

    if ((itc_copy_from_hub(ITC_ID(vehicle_pos_ctrl_z), &pos_ctrl_z) == 0 && pos_ctrl_z.is_active_z)
        && itc_copy_from_hub(ITC_ID(vehicle_local_position), &local_position) == 0) {
        const struct log_PSCD pkt = {
            APLOG_PACKET_HEADER_INIT(LOG_PSCD_MSG),
            .time_us       = pos_ctrl_z.timestamp_us,
            .pos_target    = -pos_ctrl_z.target_pos_z * 0.01f,
            .pos           = local_position.z,
            .vel_desired   = -pos_ctrl_z.vel_desired_z * 0.01f,
            .vel_target    = -pos_ctrl_z.target_vel_z * 0.01f,
            .vel           = local_position.vz,
            .accel_desired = -pos_ctrl_z.accel_desired_z * 0.01f,
            .accel_target  = -pos_ctrl_z.target_acc_z * 0.01f,
            .accel         = local_position.az
        };
        blog_write_critical_block(&pkt, sizeof(pkt));
    }
}

// Write an RCIN packet
void aplog_write_rcin()
{
    uitc_actuator_rc actuator_rc = {0};

    if (itc_copy_from_hub(ITC_ID(vehicle_rc), &actuator_rc) == 0) {
        const struct log_RCIN pkt = {
            APLOG_PACKET_HEADER_INIT(LOG_RCIN_MSG),
            .time_us       = actuator_rc.timestamp_us,
            .chan1         = actuator_rc.channels[0],
            .chan2         = actuator_rc.channels[1],
            .chan3         = actuator_rc.channels[2],
            .chan4         = actuator_rc.channels[3],
            .chan5         = actuator_rc.channels[4],
            .chan6         = actuator_rc.channels[5],
            .chan7         = actuator_rc.channels[6],
            .chan8         = actuator_rc.channels[7],
            .chan9         = actuator_rc.channels[8],
            .chan10        = actuator_rc.channels[9],
            .chan11        = actuator_rc.channels[10],
            .chan12        = actuator_rc.channels[11],
            .chan13        = actuator_rc.channels[12],
            .chan14        = actuator_rc.channels[13]
        };

        blog_write_critical_block(&pkt, sizeof(pkt));

        const struct log_RCIN2 pkt2 = {
            APLOG_PACKET_HEADER_INIT(LOG_RCIN2_MSG),
            .time_us        = actuator_rc.timestamp_us,
            .chan15         = actuator_rc.channels[14],
            .chan16         = actuator_rc.channels[15],
            .override_mask  = actuator_rc.override_mask,
        };
        blog_write_critical_block(&pkt2, sizeof(pkt2));
    }
}

// Write an SERVO packet
void aplog_write_rcout(void)
{
    uitc_actuator_outputs srv_out;

    if (itc_copy_from_hub(ITC_ID(vehicle_actuator_outputs), &srv_out) == 0) {
        const struct log_RCOUT pkt = {
            APLOG_PACKET_HEADER_INIT(LOG_RCOUT_MSG),
            .time_us       = srv_out.timestamp_us,
            .chan1         = srv_out.output[0],
            .chan2         = srv_out.output[1],
            .chan3         = srv_out.output[2],
            .chan4         = srv_out.output[3],
            .chan5         = srv_out.output[4],
            .chan6         = srv_out.output[5],
            .chan7         = srv_out.output[6],
            .chan8         = srv_out.output[7],
            .chan9         = srv_out.output[8],
            .chan10        = srv_out.output[9],
            .chan11        = srv_out.output[10],
            .chan12        = srv_out.output[11],
            .chan13        = srv_out.output[12],
            .chan14        = srv_out.output[13]
        };
        blog_write_critical_block(&pkt, sizeof(pkt));
    }
}

// Write a mode packet.
bool aplog_write_mode(uint8_t mode, const uint8_t reason)
{
    const struct log_Mode pkt = {
        APLOG_PACKET_HEADER_INIT(LOG_MODE_MSG),
        .time_us  = time_micros64(),
        .mode     = mode,
        .mode_num = mode,
        .mode_reason = reason
    };
    return blog_write_critical_block(&pkt, sizeof(pkt));
}

void aplog_write_event(enum LogEvent event_id)
{
    const struct log_Event pkt = {
        APLOG_PACKET_HEADER_INIT(LOG_EVENT_MSG),
        .time_us  = time_micros64(),
        .id       = (uint8_t)event_id
    };
    blog_write_critical_block(&pkt, sizeof(pkt));
}

bool aplog_write_mission_cmd(mission_command_const_t cmd)
{
    mavlink_mission_item_int_t mav_cmd = {0};
    mission_mission_cmd_to_mavlink_int(cmd, &mav_cmd);

    const struct log_Cmd pkt = {
        APLOG_PACKET_HEADER_INIT(LOG_CMD_MSG),
        .time_us         = time_micros64(),
        .command_total   = mission_get_singleton()->_cmd_total,
        .sequence        = mav_cmd.seq,
        .command         = mav_cmd.command,
        .param1          = mav_cmd.param1,
        .param2          = mav_cmd.param2,
        .param3          = mav_cmd.param3,
        .param4          = mav_cmd.param4,
        .latitude        = mav_cmd.x,
        .longitude       = mav_cmd.y,
        .altitude        = mav_cmd.z,
        .frame           = mav_cmd.frame
    };
    return aplog.startup_messages_finished ? blog_write_critical_block(&pkt, sizeof(pkt)) : blog_write_file(&pkt, sizeof(pkt));
}

void aplog_write_ekf_innovation()
{
    uitc_estimator_innovations innovations;

    if (itc_copy_from_hub(ITC_ID(estimator_innovations), &innovations) == 0) {
        const struct log_MKF0 pkt = {
            APLOG_PACKET_HEADER_INIT(LOG_MKF0_MSG),
            .time_us       = innovations.timestamp_sample,
            .core          = 0,
            .gps_hvel[0]   = innovations.gps_hvel[0] * 100,
            .gps_hvel[1]   = innovations.gps_hvel[1] * 100,
            .gps_vvel      = innovations.gps_vvel * 100,
            .gps_hpos[0]   = innovations.gps_hpos[0] * 100,
            .gps_hpos[1]   = innovations.gps_hpos[1] * 100,
            .gps_vpos      = innovations.gps_vpos * 100,
            .ev_hvel[0]    = innovations.ev_hvel[0] * 100,
            .ev_hvel[1]    = innovations.ev_hvel[1] * 100,
            .ev_vvel       = innovations.ev_vvel * 100,
            .ev_hpos[0]    = innovations.ev_hpos[0] * 100,
            .ev_hpos[1]    = innovations.ev_hpos[1] * 100,
            .ev_vpos       = innovations.ev_vpos * 100
        };
        blog_write_critical_block(&pkt, sizeof(pkt));

        const struct log_MKF1 pkt2 = {
            APLOG_PACKET_HEADER_INIT(LOG_MKF1_MSG),
            .time_us       = innovations.timestamp_sample,
            .core          = 0,
            .rng_vpos      = innovations.rng_vpos * 100,
            .baro_vpos     = innovations.baro_vpos * 100,
            .aux_hvel[0]   = innovations.aux_hvel[0] * 100,
            .aux_hvel[1]   = innovations.aux_hvel[1] * 100,
            .aux_vvel      = innovations.aux_vvel * 100,
            .flow[0]       = innovations.flow[0] * 100,
            .flow[1]       = innovations.flow[1] * 100,
            .heading       = innovations.heading * 100,
            .mag_field[0]  = innovations.mag_field[0] * 100,
            .mag_field[1]  = innovations.mag_field[1] * 100,
            .mag_field[2]  = innovations.mag_field[2] * 100
        };
        blog_write_critical_block(&pkt2, sizeof(pkt2));

        const struct log_MKF2 pkt3 = {
            APLOG_PACKET_HEADER_INIT(LOG_MKF2_MSG),
            .time_us       = innovations.timestamp_sample,
            .core          = 0,
            .drag[0]       = innovations.drag[0] * 100,
            .drag[1]       = innovations.drag[0] * 100,
            .airspeed      = innovations.airspeed * 100,
            .beta          = innovations.beta * 100,
            .hagl          = innovations.hagl * 100,
            .hagl_rate     = innovations.hagl_rate * 100
        };
        blog_write_critical_block(&pkt3, sizeof(pkt3));
    }
}

void aplog_write_ekf_status()
{
    uitc_estimator_status status;

    if (itc_copy_from_hub(ITC_ID(estimator_status), &status) == 0) {
        const struct log_MKF3 pkt = {
            APLOG_PACKET_HEADER_INIT(LOG_MKF3_MSG),
            .time_us                  = status.timestamp_sample,
            .core                     = 0,
            .control_mode_flags       = status.control_mode_flags,
            .output_tracking_error[0] = status.output_tracking_error[0],
            .output_tracking_error[1] = status.output_tracking_error[1],
            .output_tracking_error[2] = status.output_tracking_error[2],
            .filter_fault_flags       = status.filter_fault_flags,
            .pos_horiz_accuracy       = status.pos_horiz_accuracy,
            .pos_vert_accuracy        = status.pos_vert_accuracy
        };
        blog_write_critical_block(&pkt, sizeof(pkt));

        const struct log_MKF4 pkt2 = {
            APLOG_PACKET_HEADER_INIT(LOG_MKF4_MSG),
            .time_us                  = status.timestamp_sample,
            .core                     = 0,
            .mag_test_ratio           = status.mag_test_ratio,
            .vel_test_ratio           = status.vel_test_ratio,
            .pos_test_ratio           = status.pos_test_ratio,
            .hgt_test_ratio           = status.hgt_test_ratio,
            .tas_test_ratio           = status.tas_test_ratio,
            .hagl_test_ratio          = status.hagl_test_ratio,
            .beta_test_ratio          = status.beta_test_ratio,
            .time_slip                = status.time_slip,
            .gps_check_fail_flags     = status.gps_check_fail_flags,
            .innovation_check_flags   = status.innovation_check_flags,
            .solution_status_flags    = status.solution_status_flags,
        };
        blog_write_critical_block(&pkt2, sizeof(pkt2));

        const struct log_MKF5 pkt3 = {
            APLOG_PACKET_HEADER_INIT(LOG_MKF5_MSG),
            .time_us                  = status.timestamp_sample,
            .core                     = 0,
            .reset_count_vel_ne       = status.reset_count_vel_ne,
            .reset_count_vel_d        = status.reset_count_vel_d,
            .reset_count_pos_ne       = status.reset_count_pos_ne,
            .reset_count_pod_d        = status.reset_count_pod_d,
            .reset_count_quat         = status.reset_count_quat,
            .pre_flt_fail_mag_field_disturbed = status.pre_flt_fail_mag_field_disturbed
        };
        blog_write_critical_block(&pkt3, sizeof(pkt3));
    }
}

void aplog_write_estimator_sensor_bias()
{
    uitc_estimator_sensor_bias sensor_bias;

    if (itc_copy_from_hub(ITC_ID(estimator_sensor_bias), &sensor_bias) == 0) {
        const struct log_MKF6 pkt = {
            APLOG_PACKET_HEADER_INIT(LOG_MKF6_MSG),
            .time_us                  = sensor_bias.timestamp_sample,
            .core                     = 0,
            .gyro_bias_valid          = sensor_bias.gyro_bias_valid,
            .gyro_bias[0]             = sensor_bias.gyro_bias[0],
            .gyro_bias[1]             = sensor_bias.gyro_bias[1],
            .gyro_bias[2]             = sensor_bias.gyro_bias[2],
            .gyro_bias_variance[0]    = sensor_bias.gyro_bias_variance[0],
            .gyro_bias_variance[1]    = sensor_bias.gyro_bias_variance[1],
            .gyro_bias_variance[2]    = sensor_bias.gyro_bias_variance[2]
        };
        blog_write_critical_block(&pkt, sizeof(pkt));

        const struct log_MKF7 pkt2 = {
            APLOG_PACKET_HEADER_INIT(LOG_MKF7_MSG),
            .time_us                   = sensor_bias.timestamp_sample,
            .core                      = 0,
            .accel_bias_valid          = sensor_bias.accel_bias_valid,
            .accel_bias[0]             = sensor_bias.accel_bias[0],
            .accel_bias[1]             = sensor_bias.accel_bias[1],
            .accel_bias[2]             = sensor_bias.accel_bias[2],
            .accel_bias_variance[0]    = sensor_bias.accel_bias_variance[0],
            .accel_bias_variance[1]    = sensor_bias.accel_bias_variance[1],
            .accel_bias_variance[2]    = sensor_bias.accel_bias_variance[2]
        };
        blog_write_critical_block(&pkt2, sizeof(pkt2));
    }
}

void aplog_write_yaw_estimator_status()
{
    uitc_yaw_estimator_status yaw_est_test_data;

    if (itc_copy_from_hub(ITC_ID(yaw_estimator_status), &yaw_est_test_data) == 0) {
        const struct log_MKF8 pkt = {
            APLOG_PACKET_HEADER_INIT(LOG_MKF8_MSG),
            .time_us                 = yaw_est_test_data.timestamp_sample,
            .core                    = 0,
            .yaw_composite           = math_wrap_360(degrees(yaw_est_test_data.yaw_composite)),
            .yaw_composite_variance  = math_sqrtf(MAX(degrees(yaw_est_test_data.yaw_variance), 0.0f)),
            .yaw0                    = math_wrap_360(degrees(yaw_est_test_data.yaw[0])),
            .yaw1                    = math_wrap_360(degrees(yaw_est_test_data.yaw[1])),
            .yaw2                    = math_wrap_360(degrees(yaw_est_test_data.yaw[2])),
            .yaw3                    = math_wrap_360(degrees(yaw_est_test_data.yaw[3])),
            .yaw4                    = math_wrap_360(degrees(yaw_est_test_data.yaw[4])),
            .wgt0                    = yaw_est_test_data.weight[0],
            .wgt1                    = yaw_est_test_data.weight[1],
            .wgt2                    = yaw_est_test_data.weight[2],
            .wgt3                    = yaw_est_test_data.weight[3],
            .wgt4                    = yaw_est_test_data.weight[4],
        };
        blog_write_critical_block(&pkt, sizeof(pkt));

        const struct log_MKF9 pkt2 = {
            APLOG_PACKET_HEADER_INIT(LOG_MKF9_MSG),
            .time_us                 = yaw_est_test_data.timestamp_sample,
            .core                    = 0,
            .ivn0                    = yaw_est_test_data.innov_vn[0],
            .ivn1                    = yaw_est_test_data.innov_vn[1],
            .ivn2                    = yaw_est_test_data.innov_vn[2],
            .ivn3                    = yaw_est_test_data.innov_vn[3],
            .ivn4                    = yaw_est_test_data.innov_vn[4],
            .ive0                    = yaw_est_test_data.innov_ve[0],
            .ive1                    = yaw_est_test_data.innov_ve[1],
            .ive2                    = yaw_est_test_data.innov_ve[2],
            .ive3                    = yaw_est_test_data.innov_ve[3],
            .ive4                    = yaw_est_test_data.innov_ve[4],
        };
        blog_write_critical_block(&pkt2, sizeof(pkt2));
    }
}

void aplog_write_origin(enum ApLogOriginType origin_type, const Location *loc)
{
    const struct log_ORGN pkt = {
        APLOG_PACKET_HEADER_INIT(LOG_ORGN_MSG),
        .time_us     = time_micros64(),
        .origin_type = (uint8_t)origin_type,
        .latitude    = loc->lat,
        .longitude   = loc->lng,
        .altitude    = loc->alt
    };
    blog_write_critical_block(&pkt, sizeof(pkt));
}

// Write a Yaw PID packet
void aplog_write_pid(uint8_t msg_type, pid_info_const_t info)
{
    const struct log_PID pkt = {
        APLOG_PACKET_HEADER_INIT(msg_type),
        .time_us         = time_micros64(),
        .target          = info->target,
        .actual          = info->actual,
        .error           = info->error,
        .P               = info->P,
        .I               = info->I,
        .D               = info->D,
        .FF              = info->FF,
        .Dmod            = info->Dmod,
        .slew_rate       = info->slew_rate,
        .limit           = info->limit
    };
    blog_write_critical_block(&pkt, sizeof(pkt));
}

void aplog_write_airspeed()
{
    uitc_sensor_airspeed_raw airspeed_raw;
    if (itc_copy_from_hub(ITC_ID(sensor_airspeed_raw1), &airspeed_raw) == 0) {
        const struct log_ARSP pkt = {
            APLOG_PACKET_HEADER_INIT(LOG_ARSP_MSG),
            .time_us       = airspeed_raw.timestamp_us,
            .instance      = 0,
            .airspeed      = airspeed_raw.airspeed,
            .diffpressure  = airspeed_raw.diffpressure,
            .temperature   = (int16_t)(airspeed_raw.temperature * 100.0f),
            .rawpressure   = airspeed_raw.rawpressure,
            .offset        = airspeed_raw.offset,
            .use           = airspeed_raw.use,
            .healthy       = airspeed_raw.healthy,
            .health_prob   = airspeed_raw.health_prob,
            .test_ratio    = airspeed_raw.test_ratio,
            .primary       = airspeed_raw.primary
        };

        blog_write_critical_block(&pkt, sizeof(pkt));
    }

    if (itc_copy_from_hub(ITC_ID(sensor_airspeed_raw2), &airspeed_raw) == 0) {
        const struct log_ARSP pkt = {
            APLOG_PACKET_HEADER_INIT(LOG_ARSP_MSG),
            .time_us       = airspeed_raw.timestamp_us,
            .instance      = 0,
            .airspeed      = airspeed_raw.airspeed,
            .diffpressure  = airspeed_raw.diffpressure,
            .temperature   = (int16_t)(airspeed_raw.temperature * 100.0f),
            .rawpressure   = airspeed_raw.rawpressure,
            .offset        = airspeed_raw.offset,
            .use           = airspeed_raw.use,
            .healthy       = airspeed_raw.healthy,
            .health_prob   = airspeed_raw.health_prob,
            .test_ratio    = airspeed_raw.test_ratio,
            .primary       = airspeed_raw.primary
        };

        blog_write_critical_block(&pkt, sizeof(pkt));
    }
}

/*------------------------------------test------------------------------------*/


